Realtime Arm Servoing

MoveIt Servo allows you to stream End Effector (EEF) velocity commands to your manipulator and have it execute them concurrently. This enables teleoperation via a wide range of input schemes, or for other autonomous software to control the robot - in visual servoing or closed loop position control for instance.

This tutorial shows how to use MoveIt Servo to send real-time servo commands to a ROS-enabled robot. Some nice features of the servo node are singularity handling and collision checking that prevents the operator from breaking the robot.

Getting Started

If you haven’t already done so, make sure you’ve completed the steps in Getting Started.

Launching a Standalone Servo Node

MoveIt Servo provides a component node moveit_servo::ServoServer which allows you to send commands via ROS topics. The commands can come from anywhere, such as a joystick, keyboard, or other controller.

This demo was written for an Xbox 1 controller, but can be easily modified to use any controller compatible with the Joy package by modifying the joystick_servo_example.cpp file

To run the demo, make sure your controller is plugged in and can be detected by ros2 run joy joy_node. Usually this happens automatically after plugging the controller in. Then launch with

ros2 launch moveit2_tutorials

You should be able to control the arm with your controller now, with MoveIt Servo automatically avoiding singularities and collisions.

Without a Controller

If you do not have a controller, you can still try the demo using your keyboard. With the demo still running, in a new terminal, run

ros2 run moveit2_tutorials servo_keyboard_input

You will be able to use your keyboard to servo the robot. Send Cartesian commands with arrow keys and the . and ; keys. Change the Cartesian command frame with W for world and E for End-Effector. Send joint jogging commands with keys 1-7 (use R to reverse direction)

Expected Output

Note that the controller overlay here is just for demonstration purposes and is not actually included

Using the C++ Interface

Instead of launching Servo as its own component, you can include Servo in your own nodes via the C++ interface. Sending commands to the robot is very similiar in both cases, but for the C++ interface a little bit of setup for Servo is necessary. In exchange, you will be able to directly interact with Servo through its C++ API.

This basic C++ interface demo moves the robot in a predetermined way and can be launched with

ros2 launch moveit2_tutorials

An Rviz window should appear with a Panda arm and collision object. The arm will joint-jog for a few seconds before switching to a Cartesian movement. As the arm approaches the collision object, it slows and stops.

Expected Output

Entire Code

The entire code is available here


First we declare pointers to the node and publisher that will publish commands to Servo

rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_cmd_pub_;
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_cmd_pub_;
size_t count_ = 0;

Next we will set up the node, planning_scene_monitor, and collision object

int main(int argc, char** argv)
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  node_ = std::make_shared<rclcpp::Node>("servo_demo_node", node_options);

Pause for RViz to come up. This is necessary in an integrated demo with a single launch file


Create the planning_scene_monitor. We need to pass this to Servo’s constructor, and we should set it up first before initializing any collision objects

auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
auto planning_scene_monitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
    node_, "robot_description", tf_buffer, "planning_scene_monitor");

Here we make sure the planning_scene_monitor is updating in real time from the joint states topic

if (planning_scene_monitor->getPlanningScene())
  RCLCPP_ERROR(LOGGER, "Planning scene not configured");
  return EXIT_FAILURE;

These are the publishers that will send commands to MoveIt Servo. Two command types are supported: JointJog messages which will directly jog the robot in the joint space, and TwistStamped messages which will move the specified link with the commanded Cartesian velocity. In this demo, we jog the end effector link.

joint_cmd_pub_ = node_->create_publisher<control_msgs::msg::JointJog>("servo_demo_node/delta_joint_cmds", 10);
twist_cmd_pub_ = node_->create_publisher<geometry_msgs::msg::TwistStamped>("servo_demo_node/delta_twist_cmds", 10);

Next we will create a collision object in the way of the arm. As the arm is servoed towards it, it will slow down and stop before colliding

moveit_msgs::msg::CollisionObject collision_object;
collision_object.header.frame_id = "panda_link0"; = "box";

Make a box and put it in the way

shape_msgs::msg::SolidPrimitive box;
box.type = box.BOX;
box.dimensions = { 0.1, 0.4, 0.1 };
geometry_msgs::msg::Pose box_pose;
box_pose.position.x = 0.6;
box_pose.position.y = 0.0;
box_pose.position.z = 0.6;

Add the box as a collision object

collision_object.operation = collision_object.ADD;

Create the message to publish the collision object

moveit_msgs::msg::PlanningSceneWorld psw;
moveit_msgs::msg::PlanningScene ps;
ps.is_diff = true; = psw;

Publish the collision object to the planning scene

auto scene_pub = node_->create_publisher<moveit_msgs::msg::PlanningScene>("planning_scene", 10);

Initializing Servo

Servo requires a number of parameters to dictate its behavoir. These can be read automatically by using the makeServoParameters helper function

auto servo_parameters = moveit_servo::ServoParameters::makeServoParameters(node_, LOGGER);
if (!servo_parameters)
  RCLCPP_FATAL(LOGGER, "Failed to load the servo parameters");
  return EXIT_FAILURE;

Initialize the Servo C++ interface by passing a pointer to the node, the parameters, and the PSM

auto servo = std::make_unique<moveit_servo::Servo>(node_, servo_parameters, planning_scene_monitor);

You can start Servo directly using the C++ interface. If launched using the alternative ServoServer, a ROS service is used to start Servo. Before it is started, MoveIt Servo will not accept any commands or move the robot


Sending Commands

For this demo, we will use a simple ROS timer to send joint and twist commands to the robot

rclcpp::TimerBase::SharedPtr timer = node_->create_wall_timer(50ms, publishCommands);

Here is the timer callback for publishing commands. The C++ interface sends commands through internal ROS topics, just like if Servo was launched using ServoServer.

void publishCommands()

First we will publish 100 joint jogging commands. The joint_names field allows you to specify individual joints to move, at the velocity in the cooresponding velocities field. It is important that the message contains a recent timestamp, or Servo will think the command is stale and will not move the robot.

if (count_ < 100)
  auto msg = std::make_unique<control_msgs::msg::JointJog>();
  msg->header.stamp = node_->now();

After a while, we switch to publishing twist commands. The provided frame is the frame in which the twist is defined, not the robot frame that will follow the command. Again, we need a recent timestamp in the message

    auto msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
    msg->header.stamp = node_->now();
    msg->header.frame_id = "panda_link0";
    msg->twist.linear.x = 0.3;
    msg->twist.angular.z = 0.5;

We use a multithreaded executor here because Servo has concurrent processes for moving the robot and avoiding collisions

auto executor = std::make_unique<rclcpp::executors::MultiThreadedExecutor>();

Servo Overview

The following sections give some background information about MoveIt Servo and describe the first steps to set it up on your robot.

Servo includes a number of nice features:
  1. Cartesian End-Effector twist commands
  2. Joint commands
  3. Collision checking
  4. Singularity checking
  5. Joint position and velocity limits enforced
  6. Inputs are generic ROS messages

Setup on a New Robot


The bare minimum requirements for running MoveIt Servo with your robot include:
  1. A valid URDF and SRDF of the robot
  2. A controller that can accept joint positions or velocities from a ROS topic
  3. Joint encoders that provide rapid and accurate joint position feedback

Because the kinematics are handled by the core parts of MoveIt, it is recommended that you have a valid config package for your robot and you can run the demo launch file included with it.

Input Devices

The two primary inputs to MoveIt Servo are Cartesian commands and joint commands. These come into Servo as TwistStamped and JointJog messages respectively. The source of the commands can be almost anything including: gamepads, voice commands, a SpaceNav mouse, or PID controllers (e.g. for visual servoing).

Requirements for incoming command messages, regardless of input device are:
  1. TwistStamped and JointJog: need a timestamp in the header that is updated when the message is published
  2. JointJog: must have valid joint names in the joint_names field that correspond with the commands given in the displacements or velocities fields
  3. (Optional) TwistStamped: can provide an arbitrary frame_id in the header that the twist will be applied to. If empty, the default from the configs is used

Servo Configs

The demo config file shows the parameters needed for MoveIt Servo and is well documented.

Start with the parameters from the demo file, but some must be changed for your specific setup:
  1. robot_link_command_frame: Update this to be a valid frame in your robot, recommended as the planning frame or EEF frame
  2. command_in_type: Set to “unitless” if your input comes from a joystick, “speed_units” if the input will be in meters/second or radians/second
  3. command_out_topic: Change this to be the input topic of your controller
  4. command_out_type: Change this based on the type of message your controller needs
  5. publish_joint_positions and publish_joint_velocities: Change these based on what your controller needs. Note if command_out_type == std_msgs/Float64MultiArray, only one of these can be True
  6. joint_topic: Change this to be the joint_state topic for your arm, usually /joint_states
  7. move_group_name: Change this to be the name of your move group, as defined in your SRDF
  8. planning_frame: This should be the planning frame of your group

Open Source Feedback

See something that needs improvement? Please open a pull request on this GitHub page